#include "pfsm_servo.h"
#include "axisgroup.h"
#include "axis/axis.h"
#include "log/zuclog.h"
#include "errcode.h"
#include "kine/kine.h"
#include "axisgroup_robot.h"
#include "ag_config.h"
#include "rob_common/robtool.h"
#include "rob_common/usrframe.h"
#include "rt_servo_plan.h"
#include "over_plan.h"
#include "pfsm_servo_filter.h"
#include "dynamic/identify_dynamics.h"

#include <math.h>

#define MOTSERVO_INFO(fmt, ...) zuclog_info("MOTSERVO", fmt, ##__VA_ARGS__)
#define MOTSERVO_ERR(fmt, ...) zuclog_error("MOTSERVO", fmt, ##__VA_ARGS__)
#define MOTSERVO_DEBUG(fmt, ...) zuclog_debug("MOTSERVO", fmt, ##__VA_ARGS__)

using axis::Axis;
using namespace axisgroup;

PFsmServo::PFsmServo(Robot* grp) : PFsmState(grp) {}

int axisgroup::PFsmServo::enter()
{
    reset();

    // 从配置中更新滤波器参数
    auto servo_config = grp_->get_robot_active_config().servo_move;

    grp_->OverPlanItf()->active_servo(true);
    grp_->OverPlanItf()->rt_plan_servo_->servo_filter_->set_filter_type((ServoFilterIntf::FilterType)servo_config.filter_type);
    grp_->OverPlanItf()->rt_plan_servo_->servo_filter_->set_lpf_para(servo_config.lpf_cf);
    grp_->OverPlanItf()->rt_plan_servo_->servo_filter_->set_nlf_para(
        servo_config.nlf_vp, servo_config.nlf_ap, servo_config.nlf_jp, servo_config.nlf_vr, servo_config.nlf_ar, servo_config.nlf_jr);
    grp_->OverPlanItf()->rt_plan_servo_->servo_filter_->set_mmf_para(servo_config.mmf_max_buf, servo_config.mmf_ka, servo_config.mmf_kv, servo_config.mmf_kp);
    MOTSERVO_INFO("Active Servo Filter Type: %s\n", grp_->OverPlanItf()->rt_plan_servo_->servo_filter_->get_filter_name().c_str());
    // 更新限制
    grp_->OverPlanItf()->rt_plan_servo_->servo_filter_->init_servo_filter(grp_->joint_num(),
                                                                          &grp_->cmd_rst_,
                                                                          grp_->get_robot_active_config().joint_limit.vel.val,
                                                                          grp_->get_robot_active_config().joint_limit.acc.val,
                                                                          grp_->config_.servo_time_sec);
    wish_exit_ = false;
    last_wish_exit = false;
    MOTSERVO_INFO("Enter Servo Mode!\n");
    return 0;
}

void axisgroup::PFsmServo::reset()
{
    memset(&dyn_idt_, 0, sizeof(dyn_idt_));
    int joint_num = grp_->joint_num();
    auto config = grp_->get_robot_active_config().robot_model.dyn;
    for (int j = 0; j < IDENTIFY_TRAJ_NUM; j++)
    {
        for (int k = 0; k < joint_num; k++)
        {
            dyn_idt_.a1[k][j] = config.dyn_traj_para[k * 7 + j * 7 * joint_num];
            dyn_idt_.a2[k][j] = config.dyn_traj_para[k * 7 + 1 + j * 7 * joint_num];
            dyn_idt_.a3[k][j] = config.dyn_traj_para[k * 7 + 2 + j * 7 * joint_num];
            dyn_idt_.b1[k][j] = config.dyn_traj_para[k * 7 + 3 + j * 7 * joint_num];
            dyn_idt_.b2[k][j] = config.dyn_traj_para[k * 7 + 4 + j * 7 * joint_num];
            dyn_idt_.b3[k][j] = config.dyn_traj_para[k * 7 + 5 + j * 7 * joint_num];
            dyn_idt_.c[k][j] = config.dyn_traj_para[k * 7 + 6 + j * 7 * joint_num];
        }
    }
    dyn_idt_.W1_ = 2.0 * M_PI / 20;
    dyn_idt_.W2_ = 8.0 * M_PI / 20;
    dyn_idt_.W3_ = 16.0 * M_PI / 20;
    dyn_idt_.one_cycle_num = (int)(20 / grp_->config_.cyclictime_sec);
}

void axisgroup::PFsmServo::exit() { wish_exit_ = false; }

void axisgroup::PFsmServo::get_identify_traj_pos(RobJointVal& pos)
{
    for (size_t i = 0; i < grp_->joint_num(); i++)
    {
        pos[i] = dyn_idt_.a1[i][dyn_idt_.traj_id_] * cos(dyn_idt_.W1_ * dyn_idt_.time_count_ * grp_->config_.cyclictime_sec) +
                 dyn_idt_.a2[i][dyn_idt_.traj_id_] * cos(dyn_idt_.W2_ * dyn_idt_.time_count_ * grp_->config_.cyclictime_sec) +
                 dyn_idt_.a3[i][dyn_idt_.traj_id_] * cos(dyn_idt_.W3_ * dyn_idt_.time_count_ * grp_->config_.cyclictime_sec) +
                 dyn_idt_.b1[i][dyn_idt_.traj_id_] * sin(dyn_idt_.W1_ * dyn_idt_.time_count_ * grp_->config_.cyclictime_sec) +
                 dyn_idt_.b2[i][dyn_idt_.traj_id_] * sin(dyn_idt_.W2_ * dyn_idt_.time_count_ * grp_->config_.cyclictime_sec) +
                 dyn_idt_.b3[i][dyn_idt_.traj_id_] * sin(dyn_idt_.W3_ * dyn_idt_.time_count_ * grp_->config_.cyclictime_sec) + dyn_idt_.c[i][dyn_idt_.traj_id_];
        pos[i] *= 180.0 / M_PI;
    }
}

int axisgroup::PFsmServo::check_identify_time_count()
{
    if (dyn_idt_.time_count_ / dyn_idt_.one_cycle_num == dyn_idt_.traj_id_ + 1 && dyn_idt_.time_count_ % dyn_idt_.one_cycle_num == 0)
    {
        // switch to next traj
        dyn_idt_.traj_id_++;
    }
    dyn_idt_.time_count_++;
    if (dyn_idt_.traj_id_ == IDENTIFY_TRAJ_NUM)
    {
        // all traj finished
        dyn_idt_.traj_id_ = 0;
        dyn_idt_.time_count_ = 0;
        return 1;
    }
    return 0;
}

// 从servoj 队列中取数据，实际发送
int PFsmServo::update_plan()
{
    if (dynamics_identifying_status_ == DynIdentifyStatus::DYN_IN_MOV)
    {
        // AxisGroupPose cmd;
        // get_identify_traj_pos(cmd.jpos);
        // grp_->OverPlanItf()->rt_plan_servo_->push_cmd(1, 1, cmd);
        // if (check_identify_time_count())
        // {
        //     dynamics_identifying_status_ = DynIdentifyStatus::DYN_MOV_OVER;
        //     grp_->OverPlanItf()->rt_plan_servo_->pause(true);
        // }
        // temp test
        AxisGroupPose cmd;
        cmd.jpos = grp_->cmd_rst_.jpos;
        cmd.jpos[6] += sin(dyn_idt_.time_count_ * grp_->config_.cyclictime_sec) * 20;
        dyn_idt_.time_count_ += 1;
        grp_->OverPlanItf()->rt_plan_servo_->push_cmd(1, (int)(grp_->config_.cyclictime_sec / grp_->config_.servo_time_sec), cmd);
    }
    if (wish_exit_)
    {
        MOTSERVO_DEBUG("sync pos %s!", grp_->get_moving_state().servo_cmd_pos.val_str().c_str());
        grp_->cmd_rst_.jpos = grp_->get_moving_state().servo_cmd_pos;  // 这里可能由于插补造成位置跳变
        // grp_->cmd_rst_.pose = grp_->get_moving_state().  // #TODO
        grp_->set_pos_jump();
        if (grp_->OverPlanItf()->rt_plan_servo_->is_stopped())  // 这么做的目的，其实只是为了在rtplan停止后一个周期，关闭叠加，保证moton的指令已同步
        {
            MOTSERVO_DEBUG("Servo already stopped!");
            if (!last_wish_exit)
            {
                last_wish_exit = true;
            }
            else
            {
                grp_->OverPlanItf()->rt_plan_servo_->set_active(false);
            }
        }
    }
    return 0;
}

void axisgroup::PFsmServo::abort()
{
    wish_exit_ = true;
    dynamics_identifying_status_ = DynIdentifyStatus::DYN_STAND_BY;
    if (!grp_->OverPlanItf()->rt_plan_servo_.get())
    {
        return;
    }
    if (grp_->OverPlanItf()->rt_plan_servo_->is_active())
    {
        grp_->OverPlanItf()->rt_plan_servo_->pause(true);  // 先暂停，等待跳变插补周期结束
    }
}

bool axisgroup::PFsmServo::is_plan_over() { return !grp_->OverPlanItf()->rt_plan_servo_->is_active(); }

bool axisgroup::PFsmServo::is_stopping() { return wish_exit_; }
